/** @file m_bounding.cpp
    @brief Bounding geometry */

#include "math/m_bounding.h"

using namespace G3;

//==============================================================================
// Constructors
//==============================================================================
template <class T>
G3::RECT<T>::RECT (T aLeft, T aTop, T aRight, T aBottom) {
    Left = aLeft;
    Top = aTop;
    Right = aRight;
    Bottom = aBottom;
}

template <class T>
G3::RECT<T>::RECT (const VEC2<T> &Min, const VEC2<T> &Max) {
    Left = Min.x;
    Top = Min.y;
    Right = Max.x;
    Bottom = Max.y;
}

template <class T>
G3::AABB3<T>::AABB3 (T MinX, T MinY, T MinZ, T MaxX, T MaxY, T MaxZ) {
	Minimum.x = MinX; Maximum.x = MaxX;
	Minimum.y = MinY; Maximum.y = MaxY;
	Minimum.z = MinZ; Maximum.z = MaxZ;
}

template <class T>
G3::AABB3<T>::AABB3 (const VEC3<T> &Min, const VEC3<T> &Max) {
	Minimum = Min;
	Maximum = Max;
}

template <class T>
G3::BSPH3<T>::BSPH3 () {
	Radius = 0;
}

template <class T>
G3::BSPH3<T>::BSPH3 (const VEC3<T> &Pos, T R) {
	Position = Pos;
	Radius = R;
}

template <class T>
G3::BCAP3<T>::BCAP3 (const VEC3<T> &a, const VEC3<T> &b, const T &radius) {
    Start = a;
    End = b;
    Radius = radius;
}

//==============================================================================
// = operators
//==============================================================================
template <class T>
G3::RECT<T>& G3::RECT<T>::operator= (const RECT<T> &Rect) {
    Left = Rect.Left;
    Right = Rect.Right;
    Top = Rect.Top;
    Bottom = Rect.Bottom;

    return *this;
}

template <class T>
AABB3<T>& G3::AABB3<T>::operator= (const AABB3<T> &b) {
	Minimum = b.Minimum;
	Maximum = b.Maximum;

	return *this;
}

template <class T>
BSPH3<T>& G3::BSPH3<T>::operator= (const BSPH3<T> &b) {
	Position = b.Position;
	Radius = b.Radius;

	return *this;
}

template <class T>
BCAP3<T>& G3::BCAP3<T>::operator= (const BCAP3<T> &b) {
    Start = b.Start;
    End = b.End;
    Radius = b.Radius;

    return *this;
}

//==============================================================================
// *= operators
//==============================================================================
template <class T>
G3::RECT<T>& G3::RECT<T>::operator*= (const T &f) {
	Left *= f;
	Right *= f;
	Top *= f;
	Bottom *= f;

	return *this;
}

template <class T>
AABB3<T>& G3::AABB3<T>::operator*= (const T &f) {
	Minimum *= f;
	Maximum *= f;

	return *this;
}

template <class T>
BSPH3<T>& G3::BSPH3<T>::operator*= (const T &f) {
	Radius *= f;

	return *this;
}

template <class T>
BCAP3<T>& G3::BCAP3<T>::operator*= (const T &f) {
	Radius *= f;

	return *this;
}

//==============================================================================
// == operators
//==============================================================================
template <class T>
bool G3::RECT<T>::operator== (const RECT<T> &R) const {
	return (Left == R.Left && Top == R.Top &&
			Right == R.Right && Bottom == R.Bottom);
}

template <class T>
bool G3::AABB3<T>::operator== (const AABB3<T> &b) const {
	return (Minimum == b.Minimum && Maximum == b.Maximum);
}

template <class T>
bool G3::BSPH3<T>::operator== (const BSPH3<T> &b) const {
	return (Position == b.Position && Radius == b.Radius);
}

template <class T>
bool G3::BCAP3<T>::operator== (const BCAP3<T> &b) const {
	return (Start == b.Start && End == b.End && Radius == b.Radius);
}

//==============================================================================
// [] operators
//==============================================================================
template <class T>
T G3::RECT<T>::operator[] (const int ID) const {
	switch (ID) {
		case 1: return Top;
		case 2: return Right;
		case 3: return Bottom;
		default: break;
	}
	return Left;
}

//==============================================================================
// Expands the rect
//==============================================================================
template <class T>
void G3::RECT<T>::Expand (const RECT<T> &Rect) {
	Left = G3_MIN (Left, Rect.Left);
	Top = G3_MIN (Top, Rect.Top);
	Right = G3_MAX (Right, Rect.Right);
	Bottom = G3_MAX (Bottom, Rect.Bottom);
}

//==============================================================================
// Checks if the rectangle contains R
//==============================================================================
template <class T>
bool G3::RECT<T>::Contains (const RECT<T> &Rect) const {
	return (Left <= Rect.Left && Top <= Rect.Top &&
			Right >= Rect.Right && Bottom >= Rect.Bottom);
}

//==============================================================================
// Swaps rectangle axis
//==============================================================================
template <class T>
void G3::RECT<T>::SwapAxis () {
    T temp = Bottom; Bottom = Right; Right = temp;
    temp = Top; Top = Left; Left = temp;
}

//==============================================================================
// Resets the rectangle
//==============================================================================
template <class T>
void G3::RECT<T>::Reset () {
	Left = 0;
	Top = 0;
	Right = 0;
	Bottom = 0;
}

//==============================================================================
// Gets rectangle area
//==============================================================================
template <class T>
T G3::RECT<T>::Area () const {
	return (Right - Left) * (Bottom - Top);
}

//==============================================================================
// Dumps the rect into the log
// TODO:: needs template specialization to get this thing to work
//==============================================================================
template <class T>
void G3::RECT<T>::Dump (LOG *aLog) const {
    Assert (aLog, "RECT::Dump: Log argument is a NULL pointer..\n");

    aLog->Report ("(%lf, %lf, %lf, %lf)", Left, Top, Right, Bottom);
}

//==============================================================================
// Expands the bounding box by another
//==============================================================================
template <class T>
void G3::AABB3<T>::Expand (const AABB3<T> &b) {
    if (Minimum.x > b.Minimum.x)
        Minimum.x = b.Minimum.x;
    if (Minimum.y > b.Minimum.y)
        Minimum.y = b.Minimum.y;
    if (Minimum.z > b.Minimum.z)
        Minimum.z = b.Minimum.z;

    if (Maximum.x < b.Maximum.x)
        Maximum.x = b.Maximum.x;
    if (Maximum.y < b.Maximum.y)
        Maximum.y = b.Maximum.y;
    if (Maximum.z < b.Maximum.z)
        Maximum.z = b.Maximum.z;
}

template <class T>
void G3::AABB3<T>::Expand (const VEC3<T> &A, const VEC3<T> &B) {
    if (Minimum.x > A.x)
        Minimum.x = A.x;
    if (Minimum.y > A.y)
        Minimum.y = A.y;
    if (Minimum.z > A.z)
        Minimum.z = A.z;

    if (Maximum.x < B.x)
        Maximum.x = B.x;
    if (Maximum.y < B.y)
        Maximum.y = B.y;
    if (Maximum.z < B.z)
        Maximum.z = B.z;
}

template <class T>
void G3::AABB3<T>::Expand (const VEC3<T> &A) {
    if (Minimum.x > A.x)
        Minimum.x = A.x;
    if (Minimum.y > A.y)
        Minimum.y = A.y;
    if (Minimum.z > A.z)
        Minimum.z = A.z;

    if (Maximum.x < A.x)
        Maximum.x = A.x;
    if (Maximum.y < A.y)
        Maximum.y = A.y;
    if (Maximum.z < A.z)
        Maximum.z = A.z;
}

//==============================================================================
// Is another AABB in this one?
//==============================================================================
template <class T>
bool G3::AABB3<T>::Overlaps (const AABB3<T> &b) const {
    if (b.Maximum.x < Minimum.x ||
        b.Maximum.y < Minimum.y ||
        b.Maximum.z < Minimum.z ||
        b.Minimum.x > Maximum.x ||
        b.Minimum.y > Maximum.y ||
        b.Minimum.z > Maximum.z)
            return false;
    return true;
}

template <class T>
bool G3::AABB3<T>::Overlaps (const VEC3<T> &A, const VEC3<T> &B) const {
    if (B.x < Minimum.x ||
        B.y < Minimum.y ||
        B.z < Minimum.z ||
        A.x > Maximum.x ||
        A.y > Maximum.y ||
        A.z > Maximum.z)
            return false;
    return true;
}

//==============================================================================
// Finds the closest point to p on the bounding box.
//==============================================================================
template<class T>
void G3::AABB3<T>::ClosestPoint (const VEC3<T> &p, VEC3<T> &q) const {
    // For each coordinate axis, if the point coordinate is
    // outside of the AABB, clamp it to the box, else keep it as it is
    for (int i=0; i<3; i++) {
        T v = p.GetCoord (i);

        if (v < Minimum.GetCoord (i))
            v = Minimum.GetCoord (i);
        if (v > Maximum.GetCoord (i))
            v = Maximum.GetCoord (i);

        q [i] = v;
    }
}

//==============================================================================
// Calculates the distance of a point from the bounding box
//==============================================================================
template<class T>
double G3::AABB3<T>::Distance (const VEC3<T> &p) const {
    G3::VEC3<T> v;

    ClosestPoint (p, v);

    return G3::GetDistance (p, v);
}

//==============================================================================
// Calculates the volume of the bounding box
//==============================================================================
template<class T>
T G3::AABB3<T>::Volume () const {
    G3::VEC3<T> v = Maximum - Minimum;

    return (v.x * v.y * v.z);
}


//==============================================================================
// Updates the AABB by a rotation matrix
//==============================================================================
template <class T>
void G3::AABB3<T>::Update (const MATRIX<3, T> &M, const VEC3<T> &t) {
    for (int i=0; i<3; i++) {
        Minimum [i] = t.GetCoord (i);
        Maximum [i] = t.GetCoord (i);

        for (int j=0; j<3; j++) {
            T e = M.GetFromTable (i, j) * Minimum [j];
            T f = M.GetFromTable (i, j) * Maximum [j];

            if (e < f) {
                Minimum [i] += e;
                Maximum [i] += f;
            } else {
                Minimum [i] += f;
                Maximum [i] += e;
            }
        }
    }
}

//==============================================================================
// Renders the bounding box
//==============================================================================
template <class T>
void G3::AABB3<T>::Render (const COL3F Color) const {
    bool Lighting = false;
    bool Texture = false;

    glPushMatrix ();
    if (glIsEnabled (GL_TEXTURE_2D)){ glDisable (GL_TEXTURE_2D); Texture = true; }
    if (glIsEnabled (GL_LIGHTING)){ glDisable (GL_LIGHTING); Lighting = true; }

    glEnable (GL_LINE_SMOOTH);
    glLineWidth (2.0f);

	glColor3f (Color.r, Color.g, Color.b);
	glBegin (GL_LINE_STRIP);
		glVertex3d (Minimum.x, Minimum.y, Minimum.z);
		glVertex3d (Maximum.x, Minimum.y, Minimum.z);
		glVertex3d (Maximum.x, Maximum.y, Minimum.z);
		glVertex3d (Maximum.x, Maximum.y, Maximum.z);
		glVertex3d (Maximum.x, Minimum.y, Maximum.z);
		glVertex3d (Minimum.x, Minimum.y, Maximum.z);
		glVertex3d (Minimum.x, Maximum.y, Maximum.z);
		glVertex3d (Minimum.x, Maximum.y, Minimum.z);
		glVertex3d (Minimum.x, Minimum.y, Minimum.z);
	glEnd ();

	glBegin (GL_LINES);
		glVertex3d (Minimum.x, Maximum.y, Minimum.z);
		glVertex3d (Maximum.x, Maximum.y, Minimum.z);
		glVertex3d (Maximum.x, Minimum.y, Minimum.z);
		glVertex3d (Maximum.x, Minimum.y, Maximum.z);
		glVertex3d (Minimum.x, Maximum.y, Maximum.z);
		glVertex3d (Maximum.x, Maximum.y, Maximum.z);
		glVertex3d (Minimum.x, Minimum.y, Minimum.z);
		glVertex3d (Minimum.x, Minimum.y, Maximum.z);
	glEnd ();

    glLineWidth (1.0f);
    glDisable (GL_LINE_SMOOTH);

    if (Texture) { glEnable (GL_TEXTURE_2D); }
    if (Lighting) { glEnable (GL_LIGHTING); }
    glPopMatrix ();
}

//==============================================================================
// Finds the longest axis
//==============================================================================
template <class T>
byte G3::AABB3<T>::LongestAxis() const {
    G3::VEC3<T> a = Maximum - Minimum;

    if (a.x >= a.y && a.x >= a.z)
        return 0;
    else if (a.y >= a.x && a.y >= a.z)
        return 1;
    else
        return 2;
}

//==============================================================================
// Checks if a point is inside
//==============================================================================
template <class T>
bool G3::AABB3<T>::PointInside( G3::VEC3<T> &p ) const {
    if (p.x >= Minimum.x && p.x <= Maximum.x &&
        p.y >= Minimum.y && p.y <= Maximum.y &&
        p.z >= Minimum.z && p.z <= Maximum.z)
        return true;
    return false;
}

//==============================================================================
// Creates a bounding sphere from a bounding box
//==============================================================================
template <class T>
void G3::BSPH3<T>::Create (const VEC3<T> &Min, const VEC3<T> &Max) {
	VEC3<T> v = (Max - Min) / ((T) 2.0);
	Radius = (T) v.GetLength ();
	Position = Min + v;
}

//==============================================================================
// Expands the bounding sphere
//==============================================================================
template <class T>
void G3::BSPH3<T>::Expand (const BSPH3<T> &b) {
    VEC3<T> v = Position - b.Position;
    Radius += (T) v.GetLength () + b.Radius;
}

template <class T>
void G3::BSPH3<T>::Expand (const VEC3<T> &A, const VEC3<T> &B) {
    VEC3<T> v = Position - A, w = Position - B;
    Radius += (T) G3_MAX (v.GetLength (), w.GetLength ());
}

template <class T>
void G3::BSPH3<T>::Expand (const VEC3<T> &A) {
    VEC3<T> v = Position - A;
    Radius += (T) v.GetLength ();
}

//==============================================================================
// Checks if a point is inside
//==============================================================================
template <class T>
bool G3::BSPH3<T>::PointInside( G3::VEC3<T> &p ) const {
//    return (G3::Distance( Position, p ) <= Radius);
    G3::VEC3<T> sub = p - Position;
    return (sub.x * sub.x + sub.y * sub.y + sub.z * sub.z <= Radius * Radius);
}

//==============================================================================
// Checks if the bounding sphere overlaps another one
//==============================================================================
template <class T>
bool G3::BSPH3<T>::Overlaps (const BSPH3<T> &b) const {
    VEC3<T> d = Position - b.Position;
    T rs = Radius + b.Radius;

    return (d * d  <= rs * rs);
}

//==============================================================================
// Checks if a capsule overlaps another one
//==============================================================================
template <class T>
bool G3::BCAP3<T>::Overlaps (const BCAP3<T> &c) const {
    //TODO:: BCAP3 Overlaps
    return true;
}

//==============================================================================
// Checks if the bounding sphere overlaps with a capsule
//==============================================================================
template <class T>
bool G3::TestSphereCapsule (const BSPH3<T> &s, const BCAP3<T> &c) {
    // TODO:: TestSphereCapsule
    return true;
}

//==============================================================================
// Explicit template instantiation
//==============================================================================
template class G3::RECT<int>;
template class G3::RECT<long>;
template class G3::RECT<float>;
template class G3::RECT<double>;

template class AABB3<float>;
template class AABB3<double>;

template class BSPH3<float>;
template class BSPH3<double>;

template class BCAP3<float>;
template class BCAP3<double>;
